#if defined(_MSC_VER)
#pragma once
#endif

#ifndef LM_CORE_MATRIX_H
#define LM_CORE_MATRIX_H

#include "lm.h"
#include "vector3.h"
#include "vector4.h"
#include "point.h"

// http://msdn.microsoft.com/en-us/library/bb281696(v=vs.85).aspx

class Matrix4x4
{

public:
	float 
		m11, m12, m13, m14,
		m21, m22, m23, m24,
		m31, m32, m33, m34,
		m41, m42, m43, m44;

	// Implements a 4x4 matrix: m_ij - row-i and column-j entry

public:

	Matrix4x4();
	Matrix4x4(const Vector4&, const Vector4&,
		const Vector4&, const Vector4&); // Sets by columns!
	Matrix4x4(float, float, float, float,
		float, float, float, float,
		float, float, float, float,
		float, float, float, float); // Sets by columns
	Matrix4x4(float *mat);
	Matrix4x4(const Matrix4x4 &A);

	inline void SetIdentity();     // Set to the identity map
	inline void Set(const Matrix4x4&); // Set to the matrix.
	inline void Set(const Vector4&, const Vector4&,
		const Vector4&, const Vector4&);
	inline void Set(float, float, float, float,
		float, float, float, float,
		float, float, float, float,
		float, float, float, float);
	inline void Set(float *mat);
	inline void SetColumn1(float, float, float, float);
	inline void SetColumn2(float, float, float, float);
	inline void SetColumn3(float, float, float, float);
	inline void SetColumn4(float, float, float, float);
	inline void SetColumn1(const Vector4&);
	inline void SetColumn2(const Vector4&);
	inline void SetColumn3(const Vector4&);
	inline void SetColumn4(const Vector4&);
	inline Vector4 Column1() const;
	inline Vector4 Column2() const;
	inline Vector4 Column3() const;
	inline Vector4 Column4() const;
	inline bool IsIdentity() const;
	inline Matrix4x4 Inverse() const;	// return the inverse of the matrix
	inline Matrix4x4 Transpose() const;	// return the trnspost of the matrix

	inline Matrix4x4& operator+=(const Matrix4x4&);
	inline Matrix4x4& operator-=(const Matrix4x4&);
	inline Matrix4x4& operator*=(float);
	inline Matrix4x4& operator/=(float);
	inline Matrix4x4& operator*=(const Matrix4x4&);    // Matrix product
	inline Matrix4x4& operator=(const Matrix4x4 &A);
	inline bool operator==(const Matrix4x4 &A) const;
	inline bool operator!=(const Matrix4x4 &A) const;
	std::string ToString() const {
		std::ostringstream oss;
		/*oss << "Matrix : " << std::endl;
		oss << this->Column1().ToString() << std::endl
			<< this->Column2().ToString() << std::endl
			<< this->Column3().ToString() << std::endl
			<< this->Column4().ToString();*/
		oss << "Matrix [\n";
		oss << "         " << m11 << '\t' << m12 << '\t' << m13 << '\t' << m14 << std::endl;
		oss << "         " << m21 << '\t' << m22 << '\t' << m23 << '\t' << m24 << std::endl;
		oss << "         " << m31 << '\t' << m32 << '\t' << m33 << '\t' << m34 << std::endl;
		oss << "         " << m41 << '\t' << m42 << '\t' << m43 << '\t' << m44 << std::endl;
		oss << "       ]";
		return oss.str();
	}
};

// binary operators
inline Matrix4x4 operator+(const Matrix4x4&, const Matrix4x4&);
inline Matrix4x4 operator-(const Matrix4x4&);
inline Matrix4x4 operator-(const Matrix4x4&, const Matrix4x4&);
inline Matrix4x4 operator*(const Matrix4x4&, float);
inline Matrix4x4 operator*(float, const Matrix4x4&);
inline Matrix4x4 operator/(const Matrix4x4&, float);
inline Matrix4x4 operator*(const Matrix4x4&, const Matrix4x4&);

inline void Inverse(Matrix4x4 *A);		// invert a matrix
inline void Transpose(Matrix4x4 *A);	// transpose a matrix


inline Vector4 operator*(const Matrix4x4&, const Vector4&);
inline Vector3 operator*(const Matrix4x4&, const Vector3&);		
inline Point3 operator*(const Matrix4x4&, const Point3&);		// Take care with w channel, need to normalize w = 1
inline Normal operator*(const Matrix4x4&, const Normal&);		// Normal should be treat carefully. When perform transform, we need to use (M^-1)^t


inline Matrix4x4::Matrix4x4() { SetIdentity();}

inline Matrix4x4::Matrix4x4(const Vector4& u, const Vector4& v,
							const Vector4& s, const Vector4& t) 
{
	m11 = u.x;      // Column 1
	m21 = u.y;
	m31 = u.z;
	m41 = u.w;
	m12 = v.x;      // Column 2
	m22 = v.y;
	m32 = v.z;
	m42 = v.w;
	m13 = s.x;      // Column 3
	m23 = s.y;
	m33 = s.z;
	m43 = s.w;
	m14 = t.x;      // Column 4
	m24 = t.y;
	m34 = t.z;
	m44 = t.w;
}

inline Matrix4x4::Matrix4x4(float a11, float a12, float a13, float a14,
							float a21, float a22, float a23, float a24,
							float a31, float a32, float a33, float a34,
							float a41, float a42, float a43, float a44)
							// Values specified in row order!!!
{
	m11 = a11;      // Row 1
	m12 = a12;
	m13 = a13;
	m14 = a14;
	m21 = a21;      // Row 2
	m22 = a22;
	m23 = a23;
	m24 = a24;
	m31 = a31;      // Row 3
	m32 = a32;
	m33 = a33;
	m34 = a34;
	m41 = a41;      // Row 4
	m42 = a42;
	m43 = a43;
	m44 = a44;
}

inline Matrix4x4::Matrix4x4(float * mat)
{
	// Values specified in row order!!!
	memcpy(&m11, mat, 16*sizeof(float));
}
inline Matrix4x4::Matrix4x4(const Matrix4x4& A)
{
	memcpy(&m11, &A.m11, 16*sizeof(float));
}
inline void
Matrix4x4::SetIdentity()
{
	m11 = m22 = m33 = m44 = 1.0;
	m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41= m42 = m43 = 0.0;
}

inline void
Matrix4x4::Set(const Vector4& u, const Vector4& v,
			   const Vector4& s, const Vector4& t)
{
	m11 = u.x;      // Column 1
	m21 = u.y;
	m31 = u.z;
	m41 = u.w;
	m12 = v.x;      // Column 2
	m22 = v.y;
	m32 = v.z;
	m42 = v.w;
	m13 = s.x;      // Column 3
	m23 = s.y;
	m33 = s.z;
	m43 = s.w;
	m14 = t.x;      // Column 4
	m24 = t.y;
	m34 = t.z;
	m44 = t.w;
}

inline void
Matrix4x4::Set(float a11, float a12, float a13, float a14,
			   float a21, float a22, float a23, float a24,
			   float a31, float a32, float a33, float a34,
			   float a41, float a42, float a43, float a44)
			   // Values specified in row order!!!
{
	m11 = a11;      // Row 1
	m12 = a12;
	m13 = a13;
	m14 = a14;
	m21 = a21;      // Row 2
	m22 = a22;
	m23 = a23;
	m24 = a24;
	m31 = a31;      // Row 3
	m32 = a32;
	m33 = a33;
	m34 = a34;
	m41 = a41;      // Row 4
	m42 = a42;
	m43 = a43;
	m44 = a44;
}

inline void
Matrix4x4::Set(float *mat)
{
	// Values specified in row order!!!
	memcpy(&m11, mat, 16*sizeof(float));
}
inline void
Matrix4x4::Set(const Matrix4x4& M) // Set to the matrix.
{
	m11 = M.m11;
	m12 = M.m12;
	m13 = M.m13;
	m14 = M.m14;
	m21 = M.m21;
	m22 = M.m22;
	m23 = M.m23;
	m24 = M.m24;
	m31 = M.m31;
	m32 = M.m32;
	m33 = M.m33;
	m34 = M.m34;
	m41 = M.m41;
	m42 = M.m42;
	m43 = M.m43;
	m44 = M.m44;
}

inline void
Matrix4x4::SetColumn1(float x, float y, float z, float w)
{
	m11 = x; m21 = y; m31= z; m41 = w;
}

inline void
Matrix4x4::SetColumn2(float x, float y, float z, float w)
{
	m12 = x; m22 = y; m32= z; m42 = w;
}

inline void
Matrix4x4::SetColumn3(float x, float y, float z, float w)
{
	m13 = x; m23 = y; m33= z; m43 = w;
}

inline void
Matrix4x4::SetColumn4(float x, float y, float z, float w)
{
	m14 = x; m24 = y; m34= z; m44 = w;
}

inline void
Matrix4x4::SetColumn1(const Vector4& u)
{
	m11 = u.x; m21 = u.y; m31 = u.z; m41 = u.w;
}

inline void
Matrix4x4::SetColumn2(const Vector4& u)
{
	m12 = u.x; m22 = u.y; m32 = u.z; m42 = u.w;
}

inline void
Matrix4x4::SetColumn3(const Vector4& u)
{
	m13 = u.x; m23 = u.y; m33 = u.z; m43 = u.w;
}

inline void
Matrix4x4::SetColumn4(const Vector4& u)
{
	m14 = u.x; m24 = u.y; m34 = u.z; m44 = u.w;
}

Vector4
Matrix4x4::Column1() const
{
	return Vector4(m11, m21, m31, m41);
}

Vector4
Matrix4x4::Column2() const
{
	return Vector4(m12, m22, m32, m42);
}

Vector4
Matrix4x4::Column3() const
{
	return Vector4(m13, m23, m33, m43);
}

Vector4
Matrix4x4::Column4() const
{
	return Vector4(m14, m24, m34, m44);
}
inline bool Matrix4x4::IsIdentity() const
{
	#define EQAL_ONE(x) ((x) < 1.0001f && (x) > 0.9999f)
	#define EQAL_ZERO(x) ((x) < .0001f && (x) > -0.9999f)
	return (
				EQAL_ONE(m11)	&&	EQAL_ZERO(m12)	&&	EQAL_ZERO(m13)	&& EQAL_ZERO(m14) &&
				EQAL_ZERO(m21)	&&	EQAL_ONE(m22)	&&	EQAL_ZERO(m23)	&& EQAL_ZERO(m24) &&
				EQAL_ZERO(m31)	&&	EQAL_ZERO(m32)	&&	EQAL_ONE(m33)	&& EQAL_ZERO(m34) &&
				EQAL_ZERO(m41)	&&	EQAL_ZERO(m42)	&&	EQAL_ZERO(m43)	&& EQAL_ONE(m44) 
			);
	#undef EQAL_ONE
	#undef EQAL_ZERO
		
	/*	Not accurate for float	*/
	/*	
	return (
			m11 == 1.f && m12 == 0.f && m13 == 0.f && m14 == 0.f &&
			m21 == 0.f && m22 == 1.f && m23 == 0.f && m24 == 0.f &&
			m31 == 0.f && m32 == 0.f &&	m33 == 1.f && m34 == 0.f &&
			m41 == 0.f && m42 == 0.f &&	m43 == 0.f && m44 == 1.f	
			);
	*/
}
inline Matrix4x4
Matrix4x4::Transpose() const
{
	return Matrix4x4
		(
			m11, m21, m31, m41,
			m12, m22, m32, m42,
			m13, m23, m33, m43,
			m14, m24, m34, m44
		);
}

Matrix4x4
Matrix4x4::Inverse() const
{
	float Tbt34C12 = m31*m42-m32*m41;       // 2x2 subdeterminants
	float Tbt34C13 = m31*m43-m33*m41;
	float Tbt34C14 = m31*m44-m34*m41;
	float Tbt34C23 = m32*m43-m33*m42;
	float Tbt34C24 = m32*m44-m34*m42;
	float Tbt34C34 = m33*m44-m34*m43;
	float Tbt24C12 = m21*m42-m22*m41;       // 2x2 subdeterminants
	float Tbt24C13 = m21*m43-m23*m41;
	float Tbt24C14 = m21*m44-m24*m41;
	float Tbt24C23 = m22*m43-m23*m42;
	float Tbt24C24 = m22*m44-m24*m42;
	float Tbt24C34 = m23*m44-m24*m43;
	float Tbt23C12 = m21*m32-m22*m31;       // 2x2 subdeterminants
	float Tbt23C13 = m21*m33-m23*m31;
	float Tbt23C14 = m21*m34-m24*m31;
	float Tbt23C23 = m22*m33-m23*m32;
	float Tbt23C24 = m22*m34-m24*m32;
	float Tbt23C34 = m23*m34-m24*m33;

	float sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23;    // 3x3 subdeterminants
	float sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13;
	float sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12;
	float sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12;
	float sd21 = m12*Tbt34C34 - m13*Tbt34C24 + m14*Tbt34C23;    // 3x3 subdeterminants
	float sd22 = m11*Tbt34C34 - m13*Tbt34C14 + m14*Tbt34C13;
	float sd23 = m11*Tbt34C24 - m12*Tbt34C14 + m14*Tbt34C12;
	float sd24 = m11*Tbt34C23 - m12*Tbt34C13 + m13*Tbt34C12;
	float sd31 = m12*Tbt24C34 - m13*Tbt24C24 + m14*Tbt24C23;    // 3x3 subdeterminants
	float sd32 = m11*Tbt24C34 - m13*Tbt24C14 + m14*Tbt24C13;
	float sd33 = m11*Tbt24C24 - m12*Tbt24C14 + m14*Tbt24C12;
	float sd34 = m11*Tbt24C23 - m12*Tbt24C13 + m13*Tbt24C12;
	float sd41 = m12*Tbt23C34 - m13*Tbt23C24 + m14*Tbt23C23;    // 3x3 subdeterminants
	float sd42 = m11*Tbt23C34 - m13*Tbt23C14 + m14*Tbt23C13;
	float sd43 = m11*Tbt23C24 - m12*Tbt23C14 + m14*Tbt23C12;
	float sd44 = m11*Tbt23C23 - m12*Tbt23C13 + m13*Tbt23C12;

	float det = m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14;

	if(det == 0.0)
		Error("Singular matrix in MatrixInvert");

	register float detInv = 1.0 / det;

	return Matrix4x4
		(
			sd11*detInv,	-sd21*detInv,	 sd31*detInv,	-sd41*detInv,
			-sd12*detInv,	sd22*detInv,	-sd32*detInv,	sd42*detInv,
			sd13*detInv,	-sd23*detInv,	sd33*detInv,	-sd43*detInv,
			-sd14*detInv,	sd24*detInv,	-sd34*detInv,	sd44*detInv
		);

	/*********************** This one is 4x slower in release mode ************************/

	//int indxc[4], indxr[4];
	//int ipiv[4] = { 0, 0, 0, 0 };
	//float minv[4][4];
	//memcpy(minv, &m11, 4*4*sizeof(float));
	//for (int i = 0; i < 4; i++) {
	//	int irow = -1, icol = -1;
	//	float big = 0.;
	//	// Choose pivot
	//	for (int j = 0; j < 4; j++) {
	//		if (ipiv[j] != 1) {
	//			for (int k = 0; k < 4; k++) {
	//				if (ipiv[k] == 0) {
	//					if (fabsf(minv[j][k]) >= big) {
	//						big = float(fabsf(minv[j][k]));
	//						irow = j;
	//						icol = k;
	//					}
	//				}
	//				else if (ipiv[k] > 1)
	//					Error("Singular matrix in MatrixInvert");
	//			}
	//		}
	//	}
	//	++ipiv[icol];
	//	// Swap rows _irow_ and _icol_ for pivot
	//	if (irow != icol) {
	//		for (int k = 0; k < 4; ++k)
	//			swap(minv[irow][k], minv[icol][k]);
	//	}
	//	indxr[i] = irow;
	//	indxc[i] = icol;
	//	if (minv[icol][icol] == 0.)
	//		Error("Singular matrix in MatrixInvert");

	//	// Set $m[icol][icol]$ to one by scaling row _icol_ appropriately
	//	float pivinv = 1.f / minv[icol][icol];
	//	minv[icol][icol] = 1.f;
	//	for (int j = 0; j < 4; j++)
	//		minv[icol][j] *= pivinv;

	//	// Subtract this row from others to zero out their columns
	//	for (int j = 0; j < 4; j++) {
	//		if (j != icol) {
	//			float save = minv[j][icol];
	//			minv[j][icol] = 0;
	//			for (int k = 0; k < 4; k++)
	//				minv[j][k] -= minv[icol][k]*save;
	//		}
	//	}
	//}
	//// Swap columns to reflect permutation
	//for (int j = 3; j >= 0; j--) {
	//	if (indxr[j] != indxc[j]) {
	//		for (int k = 0; k < 4; k++)
	//			swap(minv[k][indxr[j]], minv[k][indxc[j]]);
	//	}
	//}
	//return Matrix4x4(&minv[0][0]);
}

inline Matrix4x4&
Matrix4x4::operator+=(const Matrix4x4& B)
{
	m11 += B.m11;
	m12 += B.m12;
	m13 += B.m13;
	m14 += B.m14;
	m21 += B.m21;
	m22 += B.m22;
	m23 += B.m23;
	m24 += B.m24;
	m31 += B.m31;
	m32 += B.m32;
	m33 += B.m33;
	m34 += B.m34;
	m41 += B.m41;
	m42 += B.m42;
	m43 += B.m43;
	m44 += B.m44;
	return *this;
}

inline Matrix4x4&
Matrix4x4::operator-=(const Matrix4x4& B)
{
	m11 -= B.m11;
	m12 -= B.m12;
	m13 -= B.m13;
	m14 -= B.m14;
	m21 -= B.m21;
	m22 -= B.m22;
	m23 -= B.m23;
	m24 -= B.m24;
	m31 -= B.m31;
	m32 -= B.m32;
	m33 -= B.m33;
	m34 -= B.m34;
	m41 -= B.m41;
	m42 -= B.m42;
	m43 -= B.m43;
	m44 -= B.m44;
	return(*this);
}

inline Matrix4x4
operator+(const Matrix4x4& A, const Matrix4x4& B)
{
	return Matrix4x4(A.m11+B.m11, A.m21+B.m21, A.m31+B.m31, A.m41+B.m41,
		A.m12+B.m12, A.m22+B.m22, A.m32+B.m32, A.m42+B.m42,
		A.m13+B.m13, A.m23+B.m23, A.m33+B.m33, A.m43+B.m43,
		A.m14+B.m14, A.m24+B.m24, A.m34+B.m34, A.m44+B.m44);
}

inline Matrix4x4
operator-(const Matrix4x4& A)
{
	return Matrix4x4(-A.m11, -A.m21, -A.m31, -A.m41,
		-A.m12, -A.m22, -A.m32, -A.m42,
		-A.m13, -A.m23, -A.m33, -A.m43,
		-A.m14, -A.m24, -A.m34, -A.m44);
}

inline Matrix4x4
operator-(const Matrix4x4& A, const Matrix4x4& B)
{
	return Matrix4x4(A.m11-B.m11, A.m21-B.m21, A.m31-B.m31, A.m41-B.m41,
		A.m12-B.m12, A.m22-B.m22, A.m32-B.m32, A.m42-B.m42,
		A.m13-B.m13, A.m23-B.m23, A.m33-B.m33, A.m43-B.m43,
		A.m14-B.m14, A.m24-B.m24, A.m34-B.m34, A.m44-B.m44);
}

inline Matrix4x4&
Matrix4x4::operator*=(float b)
{
	m11 *= b;
	m12 *= b;
	m13 *= b;
	m14 *= b;
	m21 *= b;
	m22 *= b;
	m23 *= b;
	m24 *= b;
	m31 *= b;
	m32 *= b;
	m33 *= b;
	m34 *= b;
	m41 *= b;
	m42 *= b;
	m43 *= b;
	m44 *= b;
	return *this;
}

inline Matrix4x4&
Matrix4x4::operator*=(const Matrix4x4& B)    // Matrix product
{
	float t1, t2, t3;       // temporary values
	t1 =  m11*B.m11 + m12*B.m21 + m13*B.m31 + m14*B.m41;
	t2 =  m11*B.m12 + m12*B.m22 + m13*B.m32 + m14*B.m42;
	t3 =  m11*B.m13 + m12*B.m23 + m13*B.m33 + m14*B.m43;
	m14 = m11*B.m14 + m12*B.m24 + m13*B.m34 + m14*B.m44;
	m11 = t1;
	m12 = t2;
	m13 = t3;

	t1 =  m21*B.m11 + m22*B.m21 + m23*B.m31 + m24*B.m41;
	t2 =  m21*B.m12 + m22*B.m22 + m23*B.m32 + m24*B.m42;
	t3 =  m21*B.m13 + m22*B.m23 + m23*B.m33 + m24*B.m43;
	m24 = m21*B.m14 + m22*B.m24 + m23*B.m34 + m24*B.m44;
	m21 = t1;
	m22 = t2;
	m23 = t3;

	t1 =  m31*B.m11 + m32*B.m21 + m33*B.m31 + m34*B.m41;
	t2 =  m31*B.m12 + m32*B.m22 + m33*B.m32 + m34*B.m42;
	t3 =  m31*B.m13 + m32*B.m23 + m33*B.m33 + m34*B.m43;
	m34 = m31*B.m14 + m32*B.m24 + m33*B.m34 + m34*B.m44;
	m31 = t1;
	m32 = t2;
	m33 = t3;

	t1 =  m41*B.m11 + m42*B.m21 + m43*B.m31 + m44*B.m41;
	t2 =  m41*B.m12 + m42*B.m22 + m43*B.m32 + m44*B.m42;
	t3 =  m41*B.m13 + m42*B.m23 + m43*B.m33 + m44*B.m43;
	m44 = m41*B.m14 + m42*B.m24 + m43*B.m34 + m44*B.m44;
	m41 = t1;
	m42 = t2;
	m43 = t3;
	return *this;
}

inline Matrix4x4
operator*(const Matrix4x4& A, const Matrix4x4& B) // Matrix product
{
	Matrix4x4 R;
	float t1, t2, t3;       // temporary values
	t1 =  A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31 + A.m14*B.m41;
	t2 =  A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32 + A.m14*B.m42;
	t3 =  A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33 + A.m14*B.m43;
	R.m14 = A.m11*B.m14 + A.m12*B.m24 + A.m13*B.m34 + A.m14*B.m44;
	R.m11 = t1;
	R.m12 = t2;
	R.m13 = t3;

	t1 =  A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31 + A.m24*B.m41;
	t2 =  A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32 + A.m24*B.m42;
	t3 =  A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33 + A.m24*B.m43;
	R.m24 = A.m21*B.m14 + A.m22*B.m24 + A.m23*B.m34 + A.m24*B.m44;
	R.m21 = t1;
	R.m22 = t2;
	R.m23 = t3;

	t1 =  A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31 + A.m34*B.m41;
	t2 =  A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32 + A.m34*B.m42;
	t3 =  A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33 + A.m34*B.m43;
	R.m34 = A.m31*B.m14 + A.m32*B.m24 + A.m33*B.m34 + A.m34*B.m44;
	R.m31 = t1;
	R.m32 = t2;
	R.m33 = t3;

	t1 =  A.m41*B.m11 + A.m42*B.m21 + A.m43*B.m31 + A.m44*B.m41;
	t2 =  A.m41*B.m12 + A.m42*B.m22 + A.m43*B.m32 + A.m44*B.m42;
	t3 =  A.m41*B.m13 + A.m42*B.m23 + A.m43*B.m33 + A.m44*B.m43;
	R.m44 = A.m41*B.m14 + A.m42*B.m24 + A.m43*B.m34 + A.m44*B.m44;
	R.m41 = t1;
	R.m42 = t2;
	R.m43 = t3;
	return R;
}

inline Matrix4x4
operator*(const Matrix4x4& A, float b)
{
	return(Matrix4x4(A.m11*b, A.m21*b, A.m31*b, A.m41*b,
		A.m12*b, A.m22*b, A.m32*b, A.m42*b,
		A.m13*b, A.m23*b, A.m33*b, A.m43*b,
		A.m14*b, A.m24*b, A.m34*b, A.m44*b));
}

inline Matrix4x4
operator*(float b, const Matrix4x4& A)
{
	return(Matrix4x4(A.m11*b, A.m21*b, A.m31*b, A.m41*b,
		A.m12*b, A.m22*b, A.m32*b, A.m42*b,
		A.m13*b, A.m23*b, A.m33*b, A.m43*b,
		A.m14*b, A.m24*b, A.m34*b, A.m44*b));
}

inline Matrix4x4
operator/(const Matrix4x4& A, float b)
{
	register float bInv = 1.0/b;
	return (A*bInv);
}

inline Matrix4x4&
Matrix4x4::operator/=(float b)
{
	register float bInv = 1.0/b;
	return (*this *= bInv);
}
inline Matrix4x4& Matrix4x4::operator =(const Matrix4x4 &A)
{
	memcpy(&m11, &A.m11, 16*sizeof(float));
	return *this;
}
inline bool Matrix4x4::operator==(const Matrix4x4 &A) const
{
	for(int i = 0; i < 16; i++)
		if((&m11)[i] != (&A.m11)[i])
			return false;
	return true;
}
inline bool Matrix4x4::operator!=(const Matrix4x4 &A) const
{
	for(int i = 0; i < 16; i++)
		if((&m11)[i] != (&A.m11)[i])
			return true;
	return false;
}
inline void Inverse(Matrix4x4 *A)		// invert a matrix
{
	float Tbt34C12 = A->m31*A->m42-A->m32*A->m41;       // 2x2 subdeterminants
	float Tbt34C13 = A->m31*A->m43-A->m33*A->m41;
	float Tbt34C14 = A->m31*A->m44-A->m34*A->m41;
	float Tbt34C23 = A->m32*A->m43-A->m33*A->m42;
	float Tbt34C24 = A->m32*A->m44-A->m34*A->m42;
	float Tbt34C34 = A->m33*A->m44-A->m34*A->m43;
	float Tbt24C12 = A->m21*A->m42-A->m22*A->m41;       // 2x2 subdeterminants
	float Tbt24C13 = A->m21*A->m43-A->m23*A->m41;
	float Tbt24C14 = A->m21*A->m44-A->m24*A->m41;
	float Tbt24C23 = A->m22*A->m43-A->m23*A->m42;
	float Tbt24C24 = A->m22*A->m44-A->m24*A->m42;
	float Tbt24C34 = A->m23*A->m44-A->m24*A->m43;
	float Tbt23C12 = A->m21*A->m32-A->m22*A->m31;       // 2x2 subdeterminants
	float Tbt23C13 = A->m21*A->m33-A->m23*A->m31;
	float Tbt23C14 = A->m21*A->m34-A->m24*A->m31;
	float Tbt23C23 = A->m22*A->m33-A->m23*A->m32;
	float Tbt23C24 = A->m22*A->m34-A->m24*A->m32;
	float Tbt23C34 = A->m23*A->m34-A->m24*A->m33;

	float sd11 = A->m22*Tbt34C34 - A->m23*Tbt34C24 + A->m24*Tbt34C23;    // 3x3 subdeterminants
	float sd12 = A->m21*Tbt34C34 - A->m23*Tbt34C14 + A->m24*Tbt34C13;
	float sd13 = A->m21*Tbt34C24 - A->m22*Tbt34C14 + A->m24*Tbt34C12;
	float sd14 = A->m21*Tbt34C23 - A->m22*Tbt34C13 + A->m23*Tbt34C12;
	float sd21 = A->m12*Tbt34C34 - A->m13*Tbt34C24 + A->m14*Tbt34C23;    // 3x3 subdeterminants
	float sd22 = A->m11*Tbt34C34 - A->m13*Tbt34C14 + A->m14*Tbt34C13;
	float sd23 = A->m11*Tbt34C24 - A->m12*Tbt34C14 + A->m14*Tbt34C12;
	float sd24 = A->m11*Tbt34C23 - A->m12*Tbt34C13 + A->m13*Tbt34C12;
	float sd31 = A->m12*Tbt24C34 - A->m13*Tbt24C24 + A->m14*Tbt24C23;    // 3x3 subdeterminants
	float sd32 = A->m11*Tbt24C34 - A->m13*Tbt24C14 + A->m14*Tbt24C13;
	float sd33 = A->m11*Tbt24C24 - A->m12*Tbt24C14 + A->m14*Tbt24C12;
	float sd34 = A->m11*Tbt24C23 - A->m12*Tbt24C13 + A->m13*Tbt24C12;
	float sd41 = A->m12*Tbt23C34 - A->m13*Tbt23C24 + A->m14*Tbt23C23;    // 3x3 subdeterminants
	float sd42 = A->m11*Tbt23C34 - A->m13*Tbt23C14 + A->m14*Tbt23C13;
	float sd43 = A->m11*Tbt23C24 - A->m12*Tbt23C14 + A->m14*Tbt23C12;
	float sd44 = A->m11*Tbt23C23 - A->m12*Tbt23C13 + A->m13*Tbt23C12;

	float det = A->m11*sd11 - A->m12*sd12 + A->m13*sd13 - A->m14*sd14;

	if(det == 0.0)
		Error("Singular matrix in MatrixInvert");

	register float detInv = 1.0 / det;

	A->m11 = sd11*detInv;
	A->m12 = -sd21*detInv;
	A->m13 = sd31*detInv;
	A->m14 = -sd41*detInv;
	A->m21 = -sd12*detInv;
	A->m22 = sd22*detInv;
	A->m23 = -sd32*detInv;
	A->m24 = sd42*detInv;
	A->m31 = sd13*detInv;
	A->m32 = -sd23*detInv;
	A->m33 = sd33*detInv;
	A->m34 = -sd43*detInv;
	A->m41 = -sd14*detInv;
	A->m42 = sd24*detInv;
	A->m43 = -sd34*detInv;
	A->m44 = sd44*detInv;
}
inline void Transpose(Matrix4x4 *A)	// transpose a matrix
{
	register float temp;
	temp = A->m12;
	A->m12 = A->m21;
	A->m21 = temp;
	temp = A->m13;
	A->m13 = A->m31;
	A->m31 = temp;
	temp = A->m14;
	A->m14 = A->m41;
	A->m41 = temp;
	temp = A->m23;
	A->m23 = A->m32;
	A->m32 = temp;
	temp = A->m24;
	A->m24 = A->m42;
	A->m42 = temp;
	temp = A->m34;
	A->m34 = A->m43;
	A->m43 = temp;
}

inline Vector4
operator*(const Matrix4x4& A, const Vector4& u)
{
	return Vector4(A.m11*u.x + A.m12*u.y + A.m13*u.z + A.m14*u.w,
		A.m21*u.x + A.m22*u.y + A.m23*u.z + A.m24*u.w,
		A.m31*u.x + A.m32*u.y + A.m33*u.z + A.m34*u.w,
		A.m41*u.x + A.m42*u.y + A.m43*u.z + A.m44*u.w);
}

inline Vector3
operator*(const Matrix4x4& A, const Vector3& u)
{
	// note that this ignores the fourth row in the matrix!
	// and w = 0 for vector
	return Vector3(A.m11*u.x + A.m12*u.y + A.m13*u.z,
		A.m21*u.x + A.m22*u.y + A.m23*u.z,
		A.m31*u.x + A.m32*u.y + A.m33*u.z);
}

inline Point3
operator*(const Matrix4x4& A, const Point3& u)
{
	float xp = A.m11*u.x + A.m12*u.y + A.m13*u.z + A.m14;
	float yp = A.m21*u.x + A.m22*u.y + A.m23*u.z + A.m24;
	float zp = A.m31*u.x + A.m32*u.y + A.m33*u.z + A.m34;
	float wp = A.m41*u.x + A.m42*u.y + A.m43*u.z + A.m44;
	Assert(wp != 0.0);
	if(wp == 1.)
		return Point3(xp, yp, zp);
	// else we have to normalize w = 1
	return Point3(xp, yp, zp) / wp;
}

inline Normal
operator*(const Matrix4x4& A, const Normal &n)
{
	// note that this ignores the fourth row in the matrix!
	// and w = 0 for normal
	return Normal(A.m11*n.x + A.m12*n.y + A.m13*n.z,
		A.m21*n.x + A.m22*n.y + A.m23*n.z,
		A.m31*n.x + A.m32*n.y + A.m33*n.z);
}

#endif	// LM_CORE_MATRIX_H
